function [ angle ] = KalmanRun( z )
%KALMANRUN Summary of this function goes here
%   Detailed explanation goes here
global P1;
global P2;
global x1;
global x2;

% First Init
%% Init Kalman
if (isempty(P1))
    P1 = eye(6);
end
if (isempty(P2))
    P2 = eye(6);
end
if (isempty(x1))
    x1 = [-0.0523; 0.0074; 1; z(4:6)];
end
if (isempty(x2))
    x2 = [0.52; 0.86; 0.014; z(4:6)];
end


%% Run KF1 to estimate phi & theta
% Get measurement data: Accelerometer, Gyrometer
r = 1;
z1 = z(1:6);
% Kalman update
[x1, P1] = TungDCM_KF1(x1, P1, z1, r);

%% Run KF2 to estimate psi
% Get measurement data: Magnetometer, phi & theta from above result
mx = z(7); my = z(8); mz = z(9);

theta = -asin(x1(1));
sin_theta = -x1(1);
cos_theta = cos(theta);
  
sin_phi = x1(2) / cos_theta;    % reduce computation
cos_phi = x1(3) / cos_theta;

Xh = mx * cos_theta + my * sin_theta * sin_phi + mz * sin_theta * cos_phi;
Yh = my * cos_phi - mz * sin_phi;

sin_psi = -Yh / norm([Xh,Yh]);
cos_psi = Xh / norm([Xh,Yh]);

z2 = [cos_theta * sin_psi;...
      cos_phi * cos_psi + sin_phi * sin_theta * sin_psi;...
      -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi;...
      x1(4) ; x1(5) ; x1(6)];
% Kalman update
[x2, P2] = TungDCM_KF2(x2, P2, z2);

%% Save rerult
%x_est1(:,i) = [x1(1:3); x2(4:6)];  % get update gyro from KF2
x_est1 = x1;
x_est2 = x2;
C31 = x_est1(1);
C32 = x_est1(2);
C33 = x_est1(3);
C21 = x_est2(1);
C22 = x_est2(2);
C23 = x_est2(3);

C11 = (C22 * C33 - C23 * C32);
C12 = (C23 * C31 - C21 * C33);
C13 = (C21 * C32 - C22 * C31);

theta = -asin(C31);
cos_theta = cos(theta);

if (C31 == 1)
    theta = pi/2;
    phi = 0;
    psi = phi + atan2(C12,C13);
elseif (C31 == -1)
    theta = -pi/2;
    phi = 0;    % anything
    psi = -phi + atan2(-C12,-C13);
else
    phi = atan2(C32/cos_theta , C33/cos_theta);
    psi = atan2(C21/cos_theta , C11/cos_theta);
end

angle = [phi theta psi];

end

